/**********************************************************************
 *** *                                                            * ***
 *** *  Copyright (c) 2011 Harmonia Holdings Group LLC            * ***
 *** *                                                            * ***
 *** * Permission is hereby granted to use, modify, copy, and     * ***
 *** * distribute this software and its documentation for any     * ***
 *** * purpose only without profit, provided that the above       * ***
 *** * Copyright Notice appears in all copies and that both the   * ***
 *** * Copyright Notice and this Permission Notice appears in     * ***
 *** * every copy of supporting documentation.  No title to nor   * ***
 *** * ownership of the software is transferred hereby.  The name * ***
 *** * of Harmonia Holdings Group LLC shall not be used in        * ***
 *** * advertising or publicity related to the distribution of    * ***
 *** * the software without specific, written, prior permission.  * ***
 *** * This software is provided as-delivered without expressed   * ***
 *** * or implied warranty.  Harmonia Holdings Group LLC          * ***
 *** * makes no representation about the suitability of this      * ***
 *** * software for any purpose and accepts no responsibility for * ***
 *** * its use.                                                   * ***
 *** *                                                            * ***
 *** ************************************************************** ***
 *** *                                                            * ***
 *** * This program is free software; you can redistribute it     * ***
 *** * and/or modify it under the terms of the GNU General Public * ***
 *** * License as published by the Free Software Foundation;      * ***
 *** * either version 2 of the License, or (at your option) any   * ***
 *** * later version.                                             * ***
 *** *                                                            * ***
 *** * This program is distributed in the hope that it will be    * ***
 *** * useful, but WITHOUT ANY WARRANTY; without even the implied * ***
 *** * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR    * ***
 *** * PURPOSE.  See the GNU General Public License for more      * ***
 *** * details.                                                   * ***
 *** *                                                            * ***
 *** * You should have received a copy of the GNU General Public  * ***
 *** * License along with this program; if not, write to the Free * ***
 *** * Software Foundation, Inc., 51 Franklin Street, Fifth       * ***
 *** * Floor, Boston, MA 02110-1301, USA.                         * ***
 *** *                                                            * ***
 *** * For more information: http://www.gnu.org/licenses/gpl.html * ***
 *** *                                                            * ***
 **********************************************************************/
package org.etexascode.api;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;

import org.etexascode.api.SimproJNA.SIMPRO_VehicleInjectionData;
import org.etexascode.api.SimproJNA.SIMPRO_VehicleInjectionError;
import org.etexascode.interrep.datamodel.Lane;
import org.etexascode.interrep.datamodel.LaneManager;
import org.etexascode.interrep.datamodel.SignalCommand;
import org.etexascode.interrep.datamodel.SignalIndication;
import org.etexascode.interrep.datamodel.SignalManager;
import org.etexascode.interrep.datamodel.StaticData;
import org.etexascode.interrep.datamodel.StepData;
import org.etexascode.interrep.datamodel.Vehicle;
import org.etexascode.interrep.datamodel.VehicleCommand;
import org.etexascode.interrep.datamodel.VehicleDestinationCommand;
import org.etexascode.interrep.datamodel.VehicleInjectionCommand;
import org.etexascode.interrep.datamodel.VehicleLaneChangeCommand;
import org.etexascode.interrep.datamodel.VehicleSpeedCommand;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import com.sun.jna.ptr.DoubleByReference;

/**
 * A wrapper class to hide data management logic between the eTEXAS C library 
 * and the Java SimproInterface. This class provides a mapping between data
 * obtained from the eTEXAS C library and the J2735 Message Set. 
 *
 * Part of the Public TEXAS Model Java API
 *
 * @author bbadillo
 * @author ablatt
 */
public class eTEXAS {

	/**
	 * Logger for convenience.
	 */
	private static final Logger LOGGER = LoggerFactory
			.getLogger(eTEXAS.class);

	/** M/S to MPH conversion ratio. */
	private static final double MS_TO_MPH = 2.23694;
	/**
	 * The current time step in the simulation in seconds
	 */
	private double currTime = 0.0;
	/**
	 * The current time step in the simulation
	 */
	private long currStep = 0;
	/**
	 * Whether or not the simulation is finished
	 */
	private boolean finished = false;
	/**
	 * Static TEXAS model configuration data. 
	 */
	private ModelData modelData;
	/**
	 * The list of vehicle messages on the current time step.
	 */
	private List<VehicleCommand> vehicleCommands = new ArrayList<VehicleCommand>();

	/**
	 * A reusable structure to hold the information for vehicle insertions.
	 */
	private SIMPRO_VehicleInjectionData simvid = new SIMPRO_VehicleInjectionData();
	
	/**
	 * A reusable structure to hold any errors caused by vehicle injections.
	 */
	private SIMPRO_VehicleInjectionError simvie = new SIMPRO_VehicleInjectionError();

	/**
	 * Constructor which initializes the TEXAS simulation program called SIMPRO. 
	 */
	public eTEXAS() {
		SimproInterface.initializeSIMPRO();

		modelData = new ModelData();
	}

	/**
	 * Get the static TEXAS model configuration data. 
	 * 
	 * @return The static TEXAS model configuration data. 
	 */
	public ModelData getModelData() {
		return modelData;
	}

	/**
	 * Change the signal in the simulation at some specified time.
	 *
	 * @param timeToChange The time in seconds at which point the signal shall change.
	 */
	public void changeSignal(double timeToChange) {
		SimproJNA.INSTANCE.changeSignal(timeToChange);
	}

	/**
	 * Hold the signal in the simulation an additional amount of time. 
	 *
	 * @param holdTime The additional amount of time to hold the signal. 
	 */
	public void holdSignal(double holdTime) {
		SimproJNA.INSTANCE.holdSignalChange(holdTime);
	}

	/**
	 * Get the current time into the simulation in seconds
	 *
	 * @return A decimal value representing the current time into simulation in seconds
	 */
	public double getCurrentTime() {
		return currTime;
	}

	/**
	 * Get whether or not the time step simulation is complete
	 *
	 * @return True if the simulation is complete, false if not
	 */
	public boolean isFinished() {
		return finished;
	}

	/**
	 * Get the data which is static across the entire simulation
	 * @return The static data on the simulation
	 */
	public StaticData getStaticData() {
		StaticData ret = new StaticData();

		ret.setDetectorManager(UtilsStaticData.populateDetectorManager());
		ret.setSignalManager(new SignalManager());
		ret.setLaneManager(UtilsStaticData.populateLaneManager(modelData, new MapDataRetriever()));
		ret.setMetaData(UtilsStaticData.populateMetaData(modelData));

		return ret;
	}

	/**
	 * Get the step data for step num in the simulation.
	 * 
	 * @param stepNum The number of the step the user wants data on
	 * @return The data for the step stepNum. Returns null if the simulation is finished. 
	 * @throws IllegalStateException if the step number is not the number of the next step. 
	 */
	public StepData getStepData(long stepNum) {
		LOGGER.trace("Enter step data...");
		
		if (finished) {
			return null;
		}

		DoubleByReference doublePtr = new DoubleByReference();
		LOGGER.trace("Enter simulateDT...");
		SimproJNA.INSTANCE.simulateDT(doublePtr);
		LOGGER.trace("Exit simulateDT...");
		double retTime = doublePtr.getValue();
		if (retTime < 0) {
			finished = true;
			return null;
		}

		SimproInterface.getVehicleInsertionErrors(simvie);
		
		currStep++;
		if (currStep >= modelData.getMaxDT()) {
			finished = true;
			return null;
		}
		// TODO: bbadillo - Add logic for exception when stepNum is incorrect. 
//		if(currStep != stepNum) {
//			throw new IllegalStateException(String.format("Step requested is %s but the current step is %s.", stepNum, currStep));
//		}

		this.currTime = retTime;

		StepData ret = new StepData();
		
		// This needs to happen before and after command processing.
		// Beforehand because VMS Messages need vehicle data to do processing.
		// and after because of injection of new vehicles.
		ret.setVehicleList(UtilsStepData.populateVehicles(modelData, new SimproJNA.SIMPRO_VehicleData()));
		
		// Vehicle Command Processing
		List<VehicleInjectionCommand> toInject = new LinkedList<VehicleInjectionCommand>();
		for (VehicleCommand vc : vehicleCommands) {
			if (vc instanceof VehicleInjectionCommand) {
				toInject.add((VehicleInjectionCommand)vc);
			}
			else {
				VMSMessage vms = convertVehicleCommandToVMSMessage(vc, ret);
				if (vms == null) {
					StringBuilder str = new StringBuilder();
					str.append("The following Vehicle command failed to be converted to a VMS Message: ");
					str.append("Vehicle ID: ");
					str.append(vc.getVehicleID());
					str.append(", Command ID: ");
					str.append(vc.getCommand());
					str.append(".");
					LOGGER.error(str.toString());
				} else {
					StringBuilder str = new StringBuilder();
					str.append("Vehicle command converted successfully to VMS message: ");
					str.append(vms.toString());
					LOGGER.trace(str.toString());
					if (vms.inject()) {
						LOGGER.trace("Successfully Injected VMS.");
					} else {
						LOGGER.error("Failed Injection of VMS Message.");
					}
				}
			}
		}
		SimproInterface.insertVehicles(currTime, modelData.getDTSize(), toInject, modelData.mapData, simvid);
		vehicleCommands.clear();

		ret.setVehicleList(UtilsStepData.populateVehicles(modelData, new SimproJNA.SIMPRO_VehicleData()));
		ret.setSignalList(UtilsStepData.populateSignals(modelData));
		//ret.setDetectorList(UtilsStepData.populateDetectors()); TODO: Implement populateDetector method

		
		for (int k = 0; k < simvie.ERRCNT; ++k) {
			String err = String.format("Error when inserting vehicle %d at time %f: %s", simvie.ERRIVN[k], simvie.ERRTIM[k],
					SimproInterface.mapInjectionErrorCodeToString(simvie.ERRNUM[k]));
			ret.addErrorMessage(err);
		}

		LOGGER.trace("Exit step data...");
		return ret;
	}

	public void updateSignalData(List<SignalIndication> signals) {
		// TODO: ablatt - fill in this method
	}

	public void updateVehicleData(List<Vehicle> vehicles) {
		// TODO: ablatt - fill in this method
	}

	/**
	 * Returns the list of vehicle commands.
	 * @return The list of vehicle commands.
	 */
	public List<VehicleCommand> getVehicleCommands() {
		return vehicleCommands;
	}

	/**
	 * Adds a vehicle command to the list of commands
	 * to inject during the next time step.
	 * @param command The command to add.
	 */
	public void addVehicleCommand(VehicleCommand command) {
		VehicleCommand remove = null;
		if(vehicleCommands == null) {
			vehicleCommands = new ArrayList<VehicleCommand>();
		}
		for (VehicleCommand v : vehicleCommands) {
			if (command.getVehicleID() == v.getVehicleID()
					&& command.getClass().equals(v.getClass())) {
				remove = v;
			}
		}
		vehicleCommands.remove(remove);
		vehicleCommands.add(command);
	}

	/**
	 * Converts a vehicle command to VMS message.
	 * @param command The vehicle command object.
	 * @return A VMS message.
	 */
	private VMSMessage convertVehicleCommandToVMSMessage(VehicleCommand command, StepData data) {
		// Get simulation data.
		VMSMessage message = new VMSMessage();
		StaticData stcData = getStaticData();
		LaneManager laneMgr = stcData.getLaneManager();

		// Verify that the vehicle is available to add the command.
		List<Vehicle> vehicles = data.getVehicles();
		Vehicle selected = null;
		for (Vehicle v : vehicles) {
			if (v.getVehicleID() == command.getVehicleID()) {
				selected = v;
			}
		}
		if (vehicles.size() <= 0 || selected == null) {
			return null;
		}

		// Calculate end lane and get approach of the lane the
		// vehicle is in.
		int count = 0;
		int approach = laneMgr.getLaneById(selected.getLaneID()).getApproachId();
		for (int key : laneMgr.getLanes().keySet()) {
			Lane lane = laneMgr.getLaneById(key);
			if (lane.getApproachId() == laneMgr.getLaneById(selected.getLaneID()).getApproachId()) {
				count = count + 1;
			}
		}

		// Calculate lane length.
		double width = (stcData.getMetaData().getSimWidth() - 200) / 2;

		// Set the message type.
		message.setType(VMSMessage.DRIVER_IVMS);

		// Convert command ID to the corresponding eTexas API command ID.
		// Set vehicle parameter, in this case speed.
		if (command instanceof VehicleSpeedCommand) {
			if (command.getCommand() == VehicleSpeedCommand.MAX_ACCELERATE_TO_XX) {
				message.setMessage(VMSMessage.ETEXAS_API_MAX_ACCELERATE);
			} else if (command.getCommand() == VehicleSpeedCommand.MAX_DECELERATE_TO_XX) {
				message.setMessage(VMSMessage.ETEXAS_API_MAX_DECELERATE);
			} else if (command.getCommand() == VehicleSpeedCommand.NORMAL_ACCELERATE_TO_XX) {
				message.setMessage(VMSMessage.ETEXAS_API_NORMAL_ACCELERATE);
			} else if (command.getCommand() == VehicleSpeedCommand.NORMAL_DECELERATE_TO_XX) {
				message.setMessage(VMSMessage.ETEXAS_API_NORMAL_DECELERATE);
			}
			message.setParameter((((VehicleSpeedCommand) command).getSpeed()) * MS_TO_MPH);
		} else if (command instanceof VehicleLaneChangeCommand) {
			if (command.getCommand() == VehicleLaneChangeCommand.CHANGE_LANE_LEFT) {
				message.setMessage(VMSMessage.ETEXAS_API_LEFT_LANE);
			} else if (command.getCommand() == VehicleLaneChangeCommand.CHANGE_LANE_RIGHT) {
				message.setMessage(VMSMessage.ETEXAS_API_RIGHT_LANE);
			}
			message.setParameter(0.0);
		} else if (command instanceof VehicleDestinationCommand) {
			if (command.getCommand() == VehicleDestinationCommand.STAY_STRAIGHT_AT_INTERSECTION) {
				message.setMessage(VMSMessage.NONE);
			} // NO ETEXAS API AVAILABLE.
			else if (command.getCommand() == VehicleDestinationCommand.TURN_LEFT_AT_INTERSECTION) {
				message.setMessage(VMSMessage.NONE);
			} // NO ETEXAS API AVAILABLE.
			else if (command.getCommand() == VehicleDestinationCommand.TURN_RIGHT_AT_INTERSECTION) {
				message.setMessage(VMSMessage.NONE);
			} // NO ETEXAS API AVAILABLE.
			message.setParameter(0.0);
		}

		// Set the remaining parameters.
		message.setStartTime(currTime + 1);
		message.setActiveTime(modelData.maxDT - (currTime + 1));
		message.setApproach(approach);
		message.setStartLane(1);
		message.setEndLane(count);
		message.setDistStart(0.0);
		message.setDistEnd(width);
		message.setVehicleID(command.getVehicleID());
		message.setDistType(1);
		message.setDistMean(0.0);
		message.setDistParam(0.0);
		return message;
	}

	/**
	 * Adds a signal command to the list of commands
	 * to inject during the next time step.
	 * @param command The command to add.
	 */
	public void addSignalCommand(SignalCommand command) {
		SimproJNA lib = SimproJNA.INSTANCE;
		if (command.getCommand() == SignalCommand.CHANGE_SIGNAL) {
			lib.changeSignal(command.getTime());
		} else if (command.getCommand() == SignalCommand.HOLD_SIGNAL) {
			lib.holdSignalChange(command.getTime());
		}
	}
	
	public void close() {
	} // TODO: ablatt - implement this method
}